#include <ros/ros.h>
#include <sdfs_tcp_pkg/SENSOR_DATA.h>
#include <string.h>
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <arpa/inet.h>
float bytesToFloat(const char *data)
{
	float value;
	std::memcpy(&value, data, sizeof(value)); // Copies 4 bytes into a float
	return value;
}
int findHeadIndex(unsigned char *buffer, int size)
{
	// 遍历缓冲区中的每个元素
	for (int i = 0; i < size; i++)
	{
		if ((buffer[i] == 0xAA) && (buffer[i + 1] == 0x55))
		{
			return i; // 找到后返回索引
		}
	}
	return -1; // 没有找到返回-1
}
int main(int argc, char **argv)
{
	char receivebuff[102400];
	unsigned char databuff[8192];
	ros::init(argc, argv, "sensor_publisher");
	ros::NodeHandle nh;
	ros::Publisher sensor_data_pub = nh.advertise<sdfs_tcp_pkg::SENSOR_DATA>("sensor_data", 50);
	sdfs_tcp_pkg::SENSOR_DATA sensor_data;
	int fd = socket(AF_INET, SOCK_STREAM, 0);
	if (fd == -1)
	{
		perror("socket");
		return -1;
	}
	struct sockaddr_in remoteport;
	remoteport.sin_family = AF_INET;
	remoteport.sin_port = htons(4008);
	inet_pton(AF_INET, "192.168.0.108", &remoteport.sin_addr.s_addr);
	int ret = connect(fd, (struct sockaddr *)&remoteport, sizeof(remoteport));
	if (ret == -1)
	{
		perror("connect");
		return -1;
	}
	std::string set_rate = "AT+SMPF=200\r\n";
	std::string MATRIX = "AT+DCPCU=?\r\n";
	std::string get_data_stream = "AT+GSD\r\n";

	send(fd, set_rate.c_str(), set_rate.size(), 0);
	int receive_rate = recv(fd, receivebuff, sizeof(receivebuff), 0);
	if (receive_rate > 0)
	{
		ROS_INFO_STREAM("receive_rate" << receivebuff);
	}
	send(fd, MATRIX.c_str(), MATRIX.size(), 0);
	int receive_rate1 = recv(fd, receivebuff, sizeof(receivebuff), 0);
	ROS_INFO_STREAM("MATRIX" << receivebuff);

	send(fd, get_data_stream.c_str(), get_data_stream.size(), 0);

	ros::Rate loop_rate(200);
	while (ros::ok())
	{
		int len = recv(fd, databuff, sizeof(databuff), 0);
		int headIndex = findHeadIndex(databuff, sizeof(databuff));
		float f_x = 0;
		float f_y = 0;
		float f_z = 0;
		float m_x = 0;
		float m_y = 0;
		float m_z = 0;
		memcpy(&f_x, databuff + headIndex + 6, 4);
		memcpy(&f_y, databuff + headIndex + 10, 4);
		memcpy(&f_z, databuff + headIndex + 14, 4);
		memcpy(&m_x, databuff + headIndex + 18, 4);
		memcpy(&m_y, databuff + headIndex + 22, 4);
		memcpy(&m_z, databuff + headIndex + 26, 4);

		ROS_INFO_STREAM("fx:" << f_x << "fy:" << f_y << "fz:" << f_z << "mx:" << m_x << "my:" << m_y << "mz:" << m_z);
		sensor_data.Fx = f_x;
		sensor_data.Fy = f_y;
		sensor_data.Fz = f_z;
		sensor_data.Mx = m_x;
		sensor_data.My = m_y;
		sensor_data.Mz = m_z;
		sensor_data_pub.publish(sensor_data);

		std::memset(databuff, 0, sizeof(databuff));
		loop_rate.sleep();
	}
	close(fd);
	return 0;
}